#ifndef MPC_H
#define MPC_H
#include <vector>
#include <map>
// #include <Eigen>
#include <Eigen/Core>                    // 矩阵运算
#include <Eigen/QR>
#include <bits/stdc++.h>
using namespace std;

class MPCFunction
{
    public:

        MPCFunction();
    
        // Solve the model given an initial state and user' command.
        // Return the first actuatotions.
        vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd vec_cmd, Eigen::VectorXd robot_cmd , double _speed_max, double _anguler_max);
        // predicted value
        vector<double> mpc_x;
        vector<double> mpc_y;
        vector<double> mpc_theta;

        double _mpc_totalcost;

        void LoadParams(const std::map<string, double> &params);
    
    private:
        // Parameters for mpc solver
        double _max_angvel, _max_speed, _bound_value;
        int _mpc_steps, _x_start, _y_start, _theta_start, _distance_start, _degree_start, _linvel_start, _angvel_start;
        std::map<string, double> _params;

};

#endif /* MPC_H*/
